
#ifndef __3d_mathsH__
#define __3d_mathsH__

#include "vector.h"
#include "aux-math.h"

/**
 * Triangle in 3D space given by three connected points
 * in clockwise order (?)
 */
struct Triangle
{
	tVector		vertices[3];

	Triangle (){};
	Triangle (tVector v1, tVector v2, tVector v3){
		vertices[0] = v1;
		vertices[1] = v2;
		vertices[2] = v3;
	};

	virtual ~Triangle() {};

	const tVector *getVertices() const{
		return vertices;
	}

	tVector& operator[](int i){
		return vertices[i];
	};
	tVector operator[](int i) const{
		return vertices[i];
	};
};


//// Orientation

struct Orientation3D;
struct Orientation3DOps : public MatrixOps<tScalar, 3, 3> {
	typedef Orientation3D tMatrix;

	// Specialize to keep Orientation3D type when dimension is still 3x3
	template<unsigned _M, unsigned _N> struct Redim {
		typedef typename MatrixType<tScalar, _M, _N>::tType result;
	};
	template<> struct Redim<3, 3> {
		typedef Orientation3D result;
	};
};

template <>
struct MatrixTraits<Orientation3D> : public MatrixTraits<Matrix<tScalar, 3, 3, Orientation3DOps> > {
};

struct Orientation3D : public Matrix<tScalar, 3, 3, Orientation3DOps>
{
	Orientation3D()
		: Matrix<tScalar, 3, 3, Orientation3DOps>(
			Matrix<tScalar, 3, 3, Orientation3DOps>::identity()){

	};
	Orientation3D(tScalar zero)
		: Matrix<tScalar, 3, 3, Orientation3DOps>(){
	};

	/// Implicit conversion from other matrix types
	template <class T, class Ops>
	Orientation3D(const Matrix<T,3,3,Ops>& m): Matrix<tScalar, 3, 3, Orientation3DOps>(m) {};

	Orientation3D(tVector x, tVector y, tVector z) {
		(*this)[0] = x; (*this)[1] = y; (*this)[2] = z;
	};

	Orientation3D	getDerivative(const tVector& w);

	template <class tElem>
	inline void getOpenGLMatrix(tElem *m) const {
		m[0] = (*this)[0][0];
		m[1] = (*this)[1][0];
		m[2] = (*this)[2][0];
		m[3] = 0;
		m[4] = (*this)[0][1];
		m[5] = (*this)[1][1];
		m[6] = (*this)[2][1];
		m[7] = 0;
		m[8] = (*this)[0][2];
		m[9] = (*this)[1][2];
		m[10] = (*this)[2][2];
		m[11] = 0;

		// Translation
		m[12] = 0;
		m[13] = 0;
		m[14] = 0;
		m[15] = 1;
	};

	template <class tElem>
	inline static Orientation3D FromOpenGLMatrix(const tElem *m) {
		Orientation3D ori;
		ori[0][0] = m[0];
		ori[1][0] = m[1];
		ori[2][0] = m[2];
		ori[0][1] = m[4];
		ori[1][1] = m[5];
		ori[2][1] = m[6];
		ori[0][2] = m[8];
		ori[1][2] = m[9];
		ori[2][2] = m[10];
		return ori;
	};

	static Orientation3D Rotation(const tDirection& vAxis, Angle a);
};



//// Transformation

struct Transformation3D;
struct Transformation3DOps : public MatrixOps<tScalar, 4, 4> {
	typedef Transformation3D tMatrix;

	// Specialize to keep Transformation3D type when dimension is still 4x4
	template<unsigned _M, unsigned _N> struct Redim {
		typedef typename MatrixType<tScalar, _M, _N>::tType result;
	};
	template<> struct Redim<4, 4> {
		typedef Transformation3D result;
	};
};

template <>
struct MatrixTraits<Transformation3D> : public MatrixTraits<Matrix<tScalar, 4, 4, Transformation3DOps> > {
};

struct Transformation3D : public Matrix<tScalar, 4, 4, Transformation3DOps>
{
	typedef Matrix<tScalar, 4, 4, Transformation3DOps> tMatrixType;

	inline Transformation3D(){};

	inline Transformation3D(const tVector& vTransl, const Orientation3D& o) {
		for(int i=0;i<3;i++) (*this)[i][3] = vTransl[i];
		for(int i=0;i<3;i++) for(int j=0;j<3;j++) (*this)[i][j] = o[i][j];
		(*this)[3][3] = 1.0;
	};

	/** Composition of transformations **/
	inline Transformation3D then(const Transformation3D& t) const{
		return (tMatrixType)t * *this;
	};

	inline tVector operator * (const tVector v) const {
		VectorType<tScalar, 4>::tType vTmp(v[0], v[1], v[2], 1.0);
		vTmp = (tMatrixType)(*this) * (vTmp);
		return tVector(vTmp[0], vTmp[1], vTmp[2]);
	};

	static Transformation3D scale(tScalar s) {
		Transformation3D t(identity());
		for(int i=0;i<3;i++) t[i][i] = s;
		return t;
	};

	static Transformation3D scale(const tVector& v) {
		Transformation3D t(identity());
		for(int i=0;i<3;i++) t[i][i] = v[i];
		return t;
	};

	inline tVector getTranslation() const {
		tVector v;
		for(int i=0;i<3;i++) v[i] = (*this)[i][3];
		return v;
	};
	
	inline Orientation3D getOrientation() const {
		Orientation3D o;
		for(int i=0;i<3;i++) for(int j=0;j<3;j++) o[i][j] = (*this)[i][j];
		return o;
	};

	template <class tElem>
	inline void getOpenGLMatrix(tElem *m) const {
		m[0] = (*this)[0][0];
		m[1] = (*this)[1][0];
		m[2] = (*this)[2][0];
		m[3] = (*this)[3][0];
		m[4] = (*this)[0][1];
		m[5] = (*this)[1][1];
		m[6] = (*this)[2][1];
		m[7] = (*this)[3][1];
		m[8] = (*this)[0][2];
		m[9] = (*this)[1][2];
		m[10] = (*this)[2][2];
		m[11] = (*this)[3][2];

		// Translation
		m[12] = (*this)[0][3];
		m[13] = (*this)[1][3];
		m[14] = (*this)[2][3];
		m[15] = (*this)[3][3];
	};

	template <class tElem>
	inline static Transformation3D FromOpenGLMatrix(const tElem *m) {
		Transformation3D t;
		t[0][0] = m[0];
		t[1][0] = m[1];
		t[2][0] = m[2];
		t[3][0] = m[3];
		t[0][1] = m[4];
		t[1][1] = m[5];
		t[2][1] = m[6];
		t[3][1] = m[7];
		t[0][2] = m[8];
		t[1][2] = m[9];
		t[2][2] = m[10];
		t[3][2] = m[11];
		t[0][3] = m[12];
		t[1][3] = m[13];
		t[2][3] = m[14];
		t[3][3] = m[15];
		return t;
	};
};

#endif
